#include "MathTool.h"
#include<iostream>

void MathTool::MultiplyMatrix(double A[4][4], double B[4][4], double D[4][4])
{
    int r, i, j;
    double C[4][4];
    for (r = 0; r < 4; r++)
        for (i = 0; i < 4; i++)
        {
            C[i][r] = 0;
            for (j = 0; j < 4; j++)
                C[i][r] += A[i][j] * B[j][r];
        }
    for (i = 0; i<4; i++)
    {
        for (j = 0; j<4; j++)
            D[i][j] = C[i][j];
    }
}

void MathTool::RotationTransformInverse4x4(double transmtx[4][4], double inverse[4][4]) {
  // int i;
  double Temp[16] = {0};
  double T[16] = {0};
  for (int i = 0; i < 4; i++)
    for (int j = 0; j < 4; j++) T[i * 4 + j] = transmtx[i][j];
  Temp[0] = T[0];
  Temp[1] = T[4];
  Temp[2] = T[8];
  Temp[3] = -(T[0] * T[3] + T[4] * T[7] + T[8] * T[11]);
  Temp[4] = T[1];
  Temp[5] = T[5];
  Temp[6] = T[9];
  Temp[7] = -(T[1] * T[3] + T[5] * T[7] + T[9] * T[11]);
  Temp[8] = T[2];
  Temp[9] = T[6];
  Temp[10] = T[10];
  Temp[11] = -(T[2] * T[3] + T[6] * T[7] + T[10] * T[11]);
  Temp[12] = 0;
  Temp[13] = 0;
  Temp[14] = 0;
  Temp[15] = 1;

  for (int i = 0; i < 4; i++)
    for (int j = 0; j < 4; j++) inverse[i][j] = Temp[i * 4 + j];
}


double MathTool::DegToRadius(double theta)
{
    double theta_rad = theta /180 * M_PI;
    return theta_rad;
}

double MathTool::RadiusToDeg(double theta)
{
    double theta_deg = theta /M_PI * 180;
    return theta_deg;
}

void MathTool::RotateAroundZ(double theta, double rotation[4][4])
{
    double rotz[4][4] = {{cos(theta), -sin(theta), 0, 0},
                         {sin(theta), cos(theta), 0, 0},
                         {0, 0, 1, 0},
                         {0, 0, 0, 1}};

    for (int m = 0; m < 4; m++)
      for (int n = 0; n < 4; n++)
        rotation[m][n] = rotz[m][n];
}


void MathTool::RotateAroundX(double theta, double rotation[4][4])
{
    double rotx[4][4] = {{1, 0, 0, 0},
                         {0, cos(theta), -sin(theta), 0},
                         {0, sin(theta), cos(theta), 0},
                         {0, 0, 0, 1}};

    for (int m = 0; m < 4; m++)
      for (int n = 0; n < 4; n++)
        rotation[m][n] = rotx[m][n];
}

void MathTool::RotateAroundY(double theta, double rotation[4][4])
{
    double roty[4][4] = {{cos(theta), 0, sin(theta), 0},
                         {0, 1, 0, 0},
                         {-sin(theta), 0, cos(theta), 0},
                         {0, 0, 0, 1}};

    for (int m = 0; m < 4; m++)
      for (int n = 0; n < 4; n++)
        rotation[m][n] = roty[m][n];
}


void MathTool::QuaternionToRotation(std::vector<double>quat, double transform_res[4][4])
{
    double q0 = quat[0];
    double q1 = quat[1];
    double q2 = quat[2];
    double q3 = quat[3];

    transform_res[0][0] = q0*q0+q1*q1-q2*q2-q3*q3;
    transform_res[0][1] = 2*(q1*q2-q0*q3);
    transform_res[0][2] = 2*(q1*q3+q0*q2);
    transform_res[0][3] = 0;

    transform_res[1][0] = 2*(q1*q2+q0*q3);
    transform_res[1][1] = q0*q0-q1*q1+q2*q2-q3*q3;
    transform_res[1][2] = 2*(q2*q3-q0*q1);
    transform_res[1][3] = 0;

    transform_res[2][0] = 2*(q1*q3-q0*q2);
    transform_res[2][1] = 2*(q2*q3+q0*q1);
    transform_res[2][2] = q0*q0-q1*q1-q2*q2+q3*q3;
    transform_res[2][3] = 0;

    transform_res[3][0] = 0;
    transform_res[3][1] = 0;
    transform_res[3][2] = 0;
    transform_res[3][3] = 1;

}

void MathTool::EulerAnglesToQuaternion(std::vector<double> rpy, std::vector<double>& quat)
{
    double rr = 0.5*rpy[0];
    double pp = 0.5*rpy[1];
    double yy = 0.5*rpy[2];
    double w = cos(rr)*cos(pp)*cos(yy) + sin(rr)*sin(pp)*sin(yy);
    quat.push_back(w);
    double x = sin(rr)*cos(pp)*cos(yy) - cos(rr)*sin(pp)*sin(yy);
    quat.push_back(x);
    double y = cos(rr)*sin(pp)*cos(yy) + sin(rr)*cos(pp)*sin(yy);
    quat.push_back(y);
    double z = cos(rr)*cos(pp)*sin(yy) - sin(rr)*sin(pp)*cos(yy);
    quat.push_back(z);
}

void MathTool::EulerAnglesToRotationByQuaternion(std::vector<double> rpy, double transform_res[4][4])
{
    std::vector<double> quat;
    EulerAnglesToQuaternion(rpy, quat);
    QuaternionToRotation(quat, transform_res);
}

void MathTool::RotationVectorToQuaternion(std::vector<double> rotation_vector, std::vector<double>& quat)
{
    double rx = rotation_vector[0];
    double ry = rotation_vector[1];
    double rz = rotation_vector[2];

    double theta = sqrt(rx*rx + ry*ry + rz*rz);
    double nx;
    double ny;
    double nz;

    if(theta == 0)
    {
        nx = 0;
        ny = 0;
        nz = 0;
    }
    else
    {
        nx = rx/theta;
        ny = ry/theta;
        nz = rz/theta;
    }


    quat.push_back(cos(0.5*theta));
    quat.push_back(nx*sin(0.5*theta));
    quat.push_back(ny*sin(0.5*theta));
    quat.push_back(nz*sin(0.5*theta));

}

void MathTool::QuaternionToRotationVector(std::vector<double> quat, std::vector<double>& rotation_vec)
{
    double theta = 2*acos(quat[0]);
    double rx, ry, rz;

    if(theta == 0)
    {
        rx = 1;
        ry = 1;
        rz = 1;
    }
    else
    {
        if(theta > M_PI)
        {
            theta = theta - 2 * M_PI;
            theta = -theta;
            rx = - quat[1]/sin(0.5*theta);
            ry = - quat[2]/sin(0.5*theta);
            rz = - quat[3]/sin(0.5*theta);
        }
        else
        {
            rx = quat[1]/sin(0.5*theta);
            ry = quat[2]/sin(0.5*theta);
            rz = quat[3]/sin(0.5*theta);
        }
    }

    rotation_vec.push_back(rx * theta);
    rotation_vec.push_back(ry * theta);
    rotation_vec.push_back(rz * theta);
}

void MathTool::RotationToEulerAngles(double transform_in[4][4], std::vector<double>& rpy)
{

    double sy = sqrt(transform_in[2][1] * transform_in[2][1] + transform_in[2][2] * transform_in[2][2]);
    double x, y, z;

    // 使用ZYX公式计算数据
    x = atan2( transform_in[2][1], transform_in[2][2]);
    y = atan2(-transform_in[2][0], sy);
    z = atan2( transform_in[1][0], transform_in[0][0]);

    // 将XYZ（Roll/Pitch/Yaw）方式加入vector中
    rpy.push_back(x);
    rpy.push_back(y);
    rpy.push_back(z);
}

void MathTool::RotationToEulerAngles_YXZ(double transform_in[4][4], std::vector<double>& rpy)
{

    double x, y, z;
    if( (transform_in[1][2] - 1.0) < 0.0001 )  // 1.0
    {
        // 不唯一
        x = -PI/2;
        y = atan2( -transform_in[0][1], transform_in[0][0] );
        z = 0.0;
    }
    else if( (transform_in[1][2] + 1.0) < 0.0001 ) // -1.0
    {
        // 不唯一
        x = PI/2;
        y = -atan2( -transform_in[0][1], transform_in[0][0] );
        z = 0.0;
    }
    else
    {
        x = asin( -transform_in[1][2] );
        y = atan2( transform_in[0][2], transform_in[2][2] );
        z = atan2( transform_in[1][0], transform_in[1][1] );
    }

    rpy.push_back(x);
    rpy.push_back(y);
    rpy.push_back(z);
}

void MathTool::TransformToRotationVector(double transform[4][4], std::vector<double>& rotation_vec)
{
    std::vector<double> rpy_vec;
    RotationToEulerAngles(transform, rpy_vec);
    std::vector<double> quat;
    EulerAnglesToQuaternion(rpy_vec, quat);
    std::vector<double> rotation_vec_tmp;
    QuaternionToRotationVector(quat, rotation_vec_tmp);

    double x = transform[0][3];
    double y = transform[1][3];
    double z = transform[2][3];

    rotation_vec.clear();

    rotation_vec.push_back(x);
    rotation_vec.push_back(y);
    rotation_vec.push_back(z);
    rotation_vec.push_back(rotation_vec_tmp[0]);
    rotation_vec.push_back(rotation_vec_tmp[1]);
    rotation_vec.push_back(rotation_vec_tmp[2]);

}

void MathTool::RotationVectorToTransform(std::vector<double> rotation_vector, double transform[4][4])
{

        double rx = rotation_vector[3];
        double ry = rotation_vector[4];
        double rz = rotation_vector[5];

        std::vector<double> rotation_vec{rx, ry, rz};
        std::vector<double> quat;
        RotationVectorToQuaternion(rotation_vec, quat);

        QuaternionToRotation(quat, transform);

        transform[0][3] = rotation_vector[0];
        transform[1][3] = rotation_vector[1];
        transform[2][3] = rotation_vector[2];
        transform[3][0] = 0;
        transform[3][1] = 0;
        transform[3][2] = 0;
        transform[3][3] = 1;
}

void MathTool::IdentifyMatrix4x4(double matrix[4][4])
{
    matrix[0][0] = 1;
    matrix[0][1] = 0;
    matrix[0][2] = 0;
    matrix[0][3] = 0;
    matrix[1][0] = 0;
    matrix[1][1] = 1;
    matrix[1][2] = 0;
    matrix[1][3] = 0;
    matrix[2][0] = 0;
    matrix[2][1] = 0;
    matrix[2][2] = 1;
    matrix[2][3] = 0;
    matrix[3][0] = 0;
    matrix[3][1] = 0;
    matrix[3][2] = 0;
    matrix[3][3] = 1;
}

void MathTool::UnifyVector(std::vector<double>& v)
{
    if(v.size() >= 3)
    {
        double length = std::sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
        if(length < 0.000001)
            return;
        v[0] /= length;
        v[1] /= length;
        v[2] /= length;
    }
}

bool MathTool::IsZeroVector(std::vector<double>& v, int size)
{
    if(v.size() != size)
        return false;
    for(int i = 0; i < size; ++i)
    {
        if( fabs(v[i]) > 0.0001 )
            return false;
    }

    return true;
}

double MathTool::ComputeAnglesByTwoVectors(std::vector<double> v1, std::vector<double> v2)
{
    if(v1.size() != 3 && v2.size() != 3)
        return -1.0;

    double dis1 = sqrt(pow(v1[0], 2)+pow(v1[1], 2)+pow(v1[2], 2));
    double dis2 = sqrt(pow(v2[0], 2)+pow(v2[1], 2)+pow(v2[2], 2));
    if(dis1 < 0.001 || dis2 < 0.001)
        return -1.0;


    // 计算夹角
    double cos_theta = ( v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2] ) / (dis1 * dis2);
    if(cos_theta < -1.0 || cos_theta > 1.0)
        return -1.0;

    double angle = std::acos(cos_theta);
    if(angle > 3.1415926/2.0)
    {
        angle = 3.1415926 - angle;
    }

    return angle;
}

// 旋转矩阵转轴角
void MathTool::RotationToAxisAngle(double transform[4][4], std::vector<double>& axis, double& angle)
{
    // 角度计算
    double matrix_tr = transform[0][0] + transform[1][1] + transform[2][2];
    angle = acos( (matrix_tr - 1) /2 );

    // 轴计算
    if(fabs(angle) < 0.0001)
    {
        axis = {0.0, 0.0, 0.0};
        return;
    }

    double factor = 1 / (2 * sin(angle));
    double v_x = factor * (transform[2][1] - transform[1][2]);
    double v_y = factor * (transform[0][2] - transform[2][0]);
    double v_z = factor * (transform[1][0] - transform[0][1]);

    axis = {v_x, v_y, v_z};
    UnifyVector(axis);
}


